package butines.core.game.steering;

import java.util.Collection;

import butines.core.game.Obstacle;
import butines.core.game.Obstacle.Intersection;
import butines.core.game.Vehicle;
import butines.core.math.Vector2f;

public class SteerLibrary {

	public Vector2f seek(Vehicle seeker, Vector2f target) {
		Vector2f position = seeker.getPosition();
		Vector2f velocity = seeker.getVelocity();
		float maxSpeed = seeker.getMaxSpeed();

		// (|target - position| * maxSpeed) - velocity
		Vector2f desiredVelocty = new Vector2f();
		desiredVelocty.sub(target, position);
		desiredVelocty.normalise();
		desiredVelocty.mult(maxSpeed);
		desiredVelocty.sub(velocity);

		return desiredVelocty;
	}

	public Vector2f flee(Vehicle fleer, Vector2f target) {
		Vector2f position = fleer.getPosition();
		Vector2f velocity = fleer.getVelocity();
		float maxSpeed = fleer.getMaxSpeed();

		// (|position - target| * maxSpeed) - velocity
		Vector2f desiredVelocty = new Vector2f();
		desiredVelocty.sub(position, target);
		desiredVelocty.normalise();
		desiredVelocty.mult(maxSpeed);
		desiredVelocty.sub(velocity);

		return desiredVelocty;
	}

	public Vector2f arrivel(Vehicle seeker, Vector2f target, float slowingDistance) {
		Vector2f position = seeker.getPosition();
		Vector2f velocity = seeker.getVelocity();
		float maxSpeed = seeker.getMaxSpeed();

		Vector2f toTarget = new Vector2f();
		toTarget.sub(target, position);
		float distance = toTarget.length();

		Vector2f desiredVelocty = new Vector2f();
		desiredVelocty.set(0, 0);
		if (distance > 0) {
			float speed = maxSpeed * (distance / slowingDistance);
			speed = Math.min(speed, maxSpeed);
			desiredVelocty.mult(toTarget, speed / distance);
			desiredVelocty.sub(velocity);
		}

		return desiredVelocty;
	}
	
	public Vector2f cohesion(Vehicle vehicle, Collection<? extends Vehicle> neighbors) {
		Vector2f steeringForce = new Vector2f();
		
		int count = 0;
		if (neighbors != null && neighbors.size() > 2) {
			for (Vehicle other : neighbors) {
				if (other == vehicle) {
					continue;
				}
				steeringForce.add(other.getPosition());
				count++;
			}
		}
		
		if (count > 0) {
			steeringForce.div(count);
			steeringForce.set(seek(vehicle, steeringForce));
		}

		return steeringForce;
	}
	

	public Vector2f separation(Vehicle vehicle, Collection<? extends Vehicle> neighbors) {
		Vector2f steeringForce = new Vector2f();

		if (neighbors == null) {
			return steeringForce;
		}
		
		for (Vehicle other : neighbors) {
			if (vehicle == other) {
				continue;
			}
			Vector2f vec = flee(vehicle, other.getPosition());

//			Vector2f vec = new Vector2f();
//			vec.sub(vehicle.getPosition(), other.getPosition());
//			float len = vec.length();	
//			vec.normalise();
//			vec.div(len);
	
			steeringForce.add(vec);
		}

		return steeringForce;
	}
	
	
	public Vector2f obstacleAvoidance(Vehicle vehicle, Collection<? extends Obstacle> obstacles) {
		Vector2f steeringForce = new Vector2f();
		
		if (obstacles == null) {
			return steeringForce;
		}
		
		Intersection nearest = null;
		Intersection next = null;
		
		for (Obstacle obstacle : obstacles) {
			if (obstacle == vehicle) {
				continue;
			}
			
			next = obstacle.intersectionWith(vehicle);
			
			if (next.intersect && (nearest == null || next.distance < nearest.distance) ) {
				nearest = next;
			}
			
		}
		
		if (nearest != null) {
			steeringForce = nearest.obstacle.steerToAvoid(vehicle);
		}

		return steeringForce;
	}
	
}
